home *** CD-ROM | disk | FTP | other *** search
/ SGI Developer Toolbox 6.1 / SGI Developer Toolbox 6.1 - Disc 4.iso / src / demos / GL / libdemo / vect.c < prev    next >
C/C++ Source or Header  |  1994-08-01  |  7KB  |  406 lines
  1. /*
  2.  * Copyright 1991, 1992, 1993, 1994, Silicon Graphics, Inc.
  3.  * All Rights Reserved.
  4.  *
  5.  * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
  6.  * the contents of this file may not be disclosed to third parties, copied or
  7.  * duplicated in any form, in whole or in part, without the prior written
  8.  * permission of Silicon Graphics, Inc.
  9.  *
  10.  * RESTRICTED RIGHTS LEGEND:
  11.  * Use, duplication or disclosure by the Government is subject to restrictions
  12.  * as set forth in subdivision (c)(1)(ii) of the Rights in Technical Data
  13.  * and Computer Software clause at DFARS 252.227-7013, and/or in similar or
  14.  * successor clauses in the FAR, DOD or NASA FAR Supplement. Unpublished -
  15.  * rights reserved under the Copyright Laws of the United States.
  16.  */
  17. /*
  18.  * vect:
  19.  *    Functions to support operations on vectors and matrices.
  20.  *
  21.  * Original code from:
  22.  * David M. Ciemiewicz, Mark Grossman, Henry Moreton, and Paul Haeberli
  23.  *
  24.  * Much mucking with by:
  25.  * Gavin Bell
  26.  */
  27. #include <stdio.h>
  28. #include "vect.h"
  29.  
  30. float *
  31. vnew()
  32. {
  33.     register float *v;
  34.  
  35.     v = (float *) malloc(sizeof(float)*3);
  36.     return v;
  37. }
  38.  
  39. float *
  40. vclone(const float *v)
  41. {
  42.     register float *c;
  43.  
  44.     c = vnew();
  45.     vcopy(v, c);
  46.     return c;
  47. }
  48.  
  49. void
  50. vcopy(const float *v1, float *v2)
  51. {
  52.     register int i;
  53.     for (i = 0 ; i < 3 ; i++)
  54.         v2[i] = v1[i];
  55. }
  56.  
  57. void
  58. vprint(const float *v)
  59. {
  60.     printf("x: %f y: %f z: %f\n",v[0],v[1],v[2]);
  61. }
  62.  
  63. void
  64. vset(float *v, float x, float y, float z)
  65. {
  66.     v[0] = x;
  67.     v[1] = y;
  68.     v[2] = z;
  69. }
  70.  
  71. void
  72. vzero(float *v)
  73. {
  74.     v[0] = 0.0;
  75.     v[1] = 0.0;
  76.     v[2] = 0.0;
  77. }
  78.  
  79. void
  80. vnormal(float *v)
  81. {
  82.     vscale(v,1.0/vlength(v));
  83. }
  84.  
  85. float
  86. vlength(const float *v)
  87. {
  88.     return fsqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
  89. }
  90.  
  91. void
  92. vscale(float *v, float div)
  93. {
  94.     v[0] *= div;
  95.     v[1] *= div;
  96.     v[2] *= div;
  97. }
  98.  
  99. void
  100. vmult(const float *src1, const float *src2, float *dst)
  101. {
  102.     dst[0] = src1[0] * src2[0];
  103.     dst[1] = src1[1] * src2[1];
  104.     dst[2] = src1[2] * src2[2];
  105. }
  106.  
  107. void
  108. vadd(const float *src1, const float *src2, float *dst)
  109. {
  110.     dst[0] = src1[0] + src2[0];
  111.     dst[1] = src1[1] + src2[1];
  112.     dst[2] = src1[2] + src2[2];
  113. }
  114.  
  115. void
  116. vsub(const float *src1, const float *src2, float *dst)
  117. {
  118.     dst[0] = src1[0] - src2[0];
  119.     dst[1] = src1[1] - src2[1];
  120.     dst[2] = src1[2] - src2[2];
  121. }
  122.  
  123. void
  124. vhalf(const float *v1, const float *v2, float *half)
  125. {
  126.     float len;
  127.  
  128.     vadd(v2,v1,half);
  129.     len = vlength(half);
  130.     if(len>0.0001)
  131.         vscale(half,1.0/len);
  132.     else
  133.         vcopy(v1, half);
  134. }
  135.  
  136. float
  137. vdot(const float *v1, const float *v2)
  138. {
  139.     return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
  140. }
  141.  
  142. void
  143. vcross(const float *v1, const float *v2, float *cross)
  144. {
  145.     float temp[3];
  146.  
  147.     temp[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
  148.     temp[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
  149.     temp[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
  150.     vcopy(temp, cross);
  151. }
  152.  
  153. void
  154. vdirection(const float *v1, float *dir)
  155. {
  156.     vcopy(v1, dir);
  157.     vnormal(dir);
  158. }
  159.  
  160. void
  161. vreflect(const float *in, const float *mirror, float *out)
  162. {
  163.     float temp[3];
  164.  
  165.     vcopy(mirror, temp);
  166.     vscale(temp,vdot(mirror,in));
  167.     vsub(temp,in,out);
  168.     vadd(temp,out,out);
  169. }
  170.  
  171. void
  172. vmultmatrix(const Matrix m1, const Matrix m2, Matrix prod)
  173. {
  174.     register int row, col;
  175.     Matrix temp;
  176.  
  177.     for(row=0 ; row<4 ; row++) 
  178.         for(col=0 ; col<4 ; col++)
  179.             temp[row][col] = m1[row][0] * m2[0][col]
  180.                            + m1[row][1] * m2[1][col]
  181.                            + m1[row][2] * m2[2][col]
  182.                            + m1[row][3] * m2[3][col];
  183.     for(row=0 ; row<4 ; row++) 
  184.         for(col=0 ; col<4 ; col++)
  185.         prod[row][col] = temp[row][col];
  186. }
  187.  
  188. void
  189. vtransform(const float *v, const Matrix mat, float *vt)
  190. {
  191.     float t[3];
  192.  
  193.     t[0] = v[0]*mat[0][0] + v[1]*mat[1][0] + v[2]*mat[2][0] + mat[3][0];
  194.     t[1] = v[0]*mat[0][1] + v[1]*mat[1][1] + v[2]*mat[2][1] + mat[3][1];
  195.     t[2] = v[0]*mat[0][2] + v[1]*mat[1][2] + v[2]*mat[2][2] + mat[3][2];
  196.     vcopy(t, vt);
  197. }
  198. void
  199. vtransform4(const float *v, const Matrix mat, float *vt)
  200. {
  201.     float t[3];
  202.  
  203.     t[0] = v[0]*mat[0][0] + v[1]*mat[1][0] + v[2]*mat[2][0] + mat[3][0];
  204.     t[1] = v[0]*mat[0][1] + v[1]*mat[1][1] + v[2]*mat[2][1] + mat[3][1];
  205.     t[2] = v[0]*mat[0][2] + v[1]*mat[1][2] + v[2]*mat[2][2] + mat[3][2];
  206.     vcopy(t, vt);
  207.     t[3] = v[0]*mat[0][3] + v[1]*mat[1][3] + v[2]*mat[2][3] + mat[3][3];
  208.     vt[3] = t[3];
  209. }
  210.  
  211. Matrix idmatrix =
  212. {
  213.     { 1.0, 0.0, 0.0, 0.0,},
  214.     { 0.0, 1.0, 0.0, 0.0,},
  215.     { 0.0, 0.0, 1.0, 0.0,},
  216.     { 0.0, 0.0, 0.0, 1.0,},
  217. };
  218.  
  219. void
  220. mcopy(const Matrix m1, Matrix m2)
  221. {
  222.     int i, j;
  223.     for (i = 0; i < 4; i++)
  224.         for (j = 0; j < 4; j++)
  225.             m2[i][j] = m1[i][j];
  226. }
  227.  
  228. void
  229. minvert(const Matrix mat, Matrix result)
  230. {
  231.     int i, j, k;
  232.     double temp;
  233.     double m[8][4];
  234.     /*   Declare identity matrix   */
  235.  
  236.     mcopy(idmatrix, result);
  237.     for (i = 0;  i < 4;  i++) {
  238.         for (j = 0;  j < 4;  j++) {
  239.             m[i][j] = mat[i][j];
  240.             m[i+4][j] = result[i][j];
  241.         }
  242.     }
  243.  
  244.     /*   Work across by columns   */
  245.  
  246.     for (i = 0;  i < 4;  i++) {
  247.         for (j = i;  (m[i][j] == 0.0) && (j < 4);  j++)
  248.                 ;
  249.         if (j == 4) {
  250.             fprintf (stderr, "error:  cannot do inverse matrix\n");
  251.             exit (2);
  252.         } 
  253.         else if (i != j) {
  254.             for (k = 0;  k < 8;  k++) {
  255.                 temp = m[k][i];   
  256.                 m[k][i] = m[k][j];   
  257.                 m[k][j] = temp;
  258.             }
  259.         }
  260.  
  261.         /*   Divide original row   */
  262.  
  263.         for (j = 7;  j >= i;  j--)
  264.             m[j][i] /= m[i][i];
  265.  
  266.         /*   Subtract other rows   */
  267.  
  268.         for (j = 0;  j < 4;  j++)
  269.             if (i != j)
  270.                 for (k = 7;  k >= i;  k--)
  271.                     m[k][j] -= m[k][i] * m[i][j];
  272.     }
  273.  
  274.     for (i = 0;  i < 4;  i++)
  275.         for (j = 0;  j < 4;  j++)
  276.                 result[i][j] = m[i+4][j];
  277. }
  278.  
  279. /*
  280.  * Get combined Model/View/Projection matrix, in any mmode
  281.  */
  282. void
  283. vgetmatrix(Matrix m)
  284. {
  285.     long mm;
  286.  
  287.     mm = getmmode();
  288.  
  289.     if (mm == MSINGLE)
  290.     {
  291.         getmatrix(m);
  292.     }
  293.     else
  294.     {
  295.         Matrix mp, mv;
  296.  
  297.         mmode(MPROJECTION);
  298.         getmatrix(mp);
  299.         mmode(MVIEWING);
  300.         getmatrix(mv);
  301.  
  302.         pushmatrix();    /* Multiply them together */
  303.         loadmatrix(mp);
  304.         multmatrix(mv);
  305.         getmatrix(m);
  306.         popmatrix();
  307.  
  308.         mmode(mm);    /* Back into the mode we started in */
  309.     }
  310. }
  311.  
  312. /*
  313.  * Gaussian Elimination with Scaled Column Pivoting
  314.  *
  315.  * copied out of the book by        Wade Olsen
  316.  *                    Silicon Graphics
  317.  *                    Feb. 12, 1990
  318.  */
  319.  
  320. void
  321. linsolve(    
  322.     const float *eqs[],     /* System of eq's to solve */
  323.     int        n,         /* of size inmat[n][n+1] */
  324.     float    *x        /* Result float *or of size x[n] */
  325. )
  326. {
  327.     int        i, j, p;
  328.  
  329.     float **a;
  330.  
  331.     /* Allocate space to work in */
  332.     /* (avoid modifying the equations passed) */
  333.     a = (float **)malloc(sizeof(float *)*n);
  334.     for (i = 0; i < n; i++)
  335.     {
  336.         a[i] = (float *)malloc(sizeof(float)*(n+1));
  337.         bcopy(eqs[i], a[i], sizeof(float)*(n+1));
  338.     }
  339.  
  340.  
  341.     if (n == 1)
  342.     {        /* The simple single variable case */
  343.         x[0] = a[0][1] / a[0][0];
  344.         return;
  345.     }
  346.                 /* Gausian elimination process */    
  347.     for (i = 0; i < n -1; i++)
  348.     {
  349.  
  350.                 /* find non-zero pivot row */
  351.         p = i;
  352.         while (a[p][i] == 0.0)
  353.         {
  354.             p++;
  355.             if (p == n)
  356.             {
  357.                 printf("linsolv:  No unique solution exists.\n");
  358.                 exit(1);
  359.             }
  360.         }
  361.                 /* row swap */
  362.         if (p != i)
  363.         {
  364.             float *swap;
  365.  
  366.             swap = a[i];
  367.             a[i] = a[p];
  368.             a[p] = swap;
  369.         }
  370.                 /* row subtractions */
  371.         for (j = i + 1; j < n; j++)
  372.         {
  373.             float mult    = a[j][i] / a[i][i];
  374.  
  375.             int    k;
  376.             for (k = i + 1; k < n + 1; k++)
  377.                 a[j][k] -= mult * a[i][k];
  378.         }
  379.     }
  380.  
  381.     if (a[n-1][n-1] == 0.0)
  382.     {
  383.         printf("linsolv:  No unique solution exists.\n");
  384.         exit(1);
  385.     }
  386.  
  387.                     /* backwards substitution */
  388.     x[n-1] = a[n-1][n] / a[n-1][n-1];
  389.     for (i = n -2; i > -1; i--)
  390.     {
  391.         float sum = a[i][n];
  392.  
  393.         for (j = i + 1; j < n; j++)
  394.             sum -= a[i][j] * x[j];
  395.  
  396.         x[i] = sum / a[i][i];
  397.     }
  398.  
  399.     /* Free working space */
  400.     for (i = 0; i < n; i++)
  401.     {
  402.         free(a[i]);
  403.     }
  404.     free(a);
  405. }
  406.